package monitor;

import gui.OpCom;
import gui.PlotData;
import se.lth.control.DoublePoint;

public class ControllerStatMonitor {
	private long startTime = System.currentTimeMillis();
	private  double beamAngle1 = 0.0;
	private  double ballPosition1 = 0.0;
	private double ballPosition0 = 0.0;
	private double controlSignal = 0.0;
	private OpCom op;
	private double ref = 0;
	private int counter;

	public ControllerStatMonitor(OpCom op) {
		this.op = op;
	}
	public  synchronized void setBeamAngle1(double beamAngle) {
		beamAngle1 = beamAngle;
	}
	public synchronized  double getBeamAngle1() {
		return beamAngle1;
	}
	public synchronized  double getBallposition1() {
		return ballPosition1;
	}
	public synchronized void setBallPosition1(double ballPosition) {
			if( counter > 4){
				ballPosition0 = ballPosition1;
				ballPosition1= ballPosition; 
				counter = 0;
				} else{ counter++;
				}
				}

	public synchronized  void setControlSignal(double u) {
		controlSignal = u;
		
	}
	public synchronized  void setRef(double u) {
		ref= u;
		
	}
	
		
	
public synchronized boolean hasReached(double reqBallPos) {
		if(reqBallPos < 0 && ballPosition1 < 0){
			return ballPosition1 <= reqBallPos ? true:false;
		}
		if(reqBallPos < 0 && ballPosition1 >= 0){
			return ballPosition1 <= reqBallPos ? true:false;
		}
		 if(reqBallPos >= 0 && ballPosition1 >= 0){
			 return ballPosition1 >= reqBallPos ? true:false;
		 }
		 if(reqBallPos >= 0 && ballPosition1 < 0){
			 return ballPosition1 >= reqBallPos ? true:false;
		 }
		 return false;
		
	}

public synchronized boolean hasPosDer(){
	
	if( ballPosition0 == 0 ||ballPosition1 == 0 ){
		return false;
	}
	return ballPosition0 - ballPosition1 > 0 ? false:true;
	
}


public synchronized void plotData() {
	double x = (double)(System.currentTimeMillis() - startTime) / 1000.0;
	DoublePoint dp = new DoublePoint(x,controlSignal);
	
	DoublePoint bp = new DoublePoint(x,ballPosition1);
	DoublePoint ap = new DoublePoint(x,beamAngle1);
	PlotData pd = new PlotData(x,ref,beamAngle1);
	op.putControlDataPoint(dp);
	op.putMeasurementDataPoint(pd);
	op.putAngleDataPoint(ap);
	op.putPosDataPoint(bp);
	op.displayAngle(beamAngle1);
	
	
	
}


	
}
